{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "sys.path.append('../scripts/')\n",
    "from sarsa import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class NstepSarsaAgent(SarsaAgent): \n",
    "    def __init__(self, time_interval, estimator, puddle_coef=100,  alpha=0.5, widths=np.array([0.2, 0.2, math.pi/18]).T, \\\n",
    "                 lowerleft=np.array([-4, -4]).T, upperright=np.array([4, 4]).T, dev_borders=[0.1,0.2,0.4,0.8], nstep=10): \n",
    "        super().__init__(time_interval, estimator, puddle_coef,  alpha, widths, lowerleft, upperright, dev_borders)\n",
    "        \n",
    "        self.s_trace = []\n",
    "        self.a_trace = []\n",
    "        self.r_trace = []\n",
    "        self.nstep = nstep\n",
    "        \n",
    "    def set_action_value_function(self): ###nstep_sarsa2agent\n",
    "        ss = {}\n",
    "        for index in self.indexes: \n",
    "            ss[index] = StateInfo(len(self.actions))\n",
    "            for i, a in enumerate(self.actions):\n",
    "                ss[index].q[i] = -1000.0\n",
    "                \n",
    "        return ss\n",
    "        \n",
    "    def decision(self, observation=None):\n",
    "        ##終了処理##\n",
    "        if self.update_end:  return 0.0, 0.0\n",
    "        if self.in_goal:          self.update_end = True\n",
    "        \n",
    "        ##カルマンフィルタの実行##\n",
    "        self.estimator.motion_update(self.prev_nu, self.prev_omega, self.time_interval)\n",
    "        self.estimator.observation_update(observation)\n",
    "        \n",
    "        ##行動決定と報酬の処理##\n",
    "        s_, a_ = self.policy(self.estimator.pose)\n",
    "        r = self.time_interval*self.reward_per_sec() \n",
    "        self.r_trace.append(r) #インデックスの整合性のためにrは先に登録しておく\n",
    "        self.total_reward += r\n",
    "        \n",
    "        ##s', a', rの記録とQ値の更新##\n",
    "        self.q_update(s_, a_, self.nstep)\n",
    "        self.s_trace.append(s_)\n",
    "        self.a_trace.append(a_)\n",
    "\n",
    "        ##出力##\n",
    "        self.prev_nu, self.prev_omega = self.actions[a_]\n",
    "        return self.actions[a_]\n",
    "\n",
    "    def q_update(self, s_, a_, n):\n",
    "        if n > len(self.s_trace) or n == 0: return\n",
    "        \n",
    "        s = self.s_trace[-n]\n",
    "        a = self.a_trace[-n]\n",
    "        \n",
    "        q = self.ss[s].q[a]\n",
    "        r = sum(self.r_trace[-n:])\n",
    "        q_ = self.final_value if self.in_goal else self.ss[s_].q[a_]\n",
    "        self.ss[s].q[a] = (1.0 - self.alpha)*q + self.alpha*(r + q_)\n",
    "        \n",
    "        if self.in_goal:\n",
    "            self.q_update(s_, a_, n-1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class WarpRobot2(WarpRobot): ###nstep_sarsa2warprobot2\n",
    "    def __init__(self, *args, **kwargs): \n",
    "        super().__init__(*args, **kwargs)\n",
    "        \n",
    "    def choose_pose(self): #初期位置を8x8[m]領域でランダムに変更\n",
    "        return np.array([random.random()*8-4, random.random()*8-4, random.random()*2*math.pi]).T\n",
    "    \n",
    "    def one_step(self, time_interval): #300ステップ（パラメータを変えるとずれるが30秒に相当）で打ち切る処理を追加\n",
    "        if self.agent.update_end:\n",
    "            with open(\"log.txt\", \"a\") as f:\n",
    "                f.write(\"{}\\n\".format(self.agent.total_reward + self.agent.final_value))\n",
    "            self.reset()\n",
    "            return\n",
    "        elif len(self.poses) >= 300:\n",
    "            with open(\"log.txt\", \"a\") as f:\n",
    "                f.write(\"DNF\\n\")\n",
    "            self.reset()\n",
    "            return\n",
    "        \n",
    "        super().one_step(time_interval)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def trial(): \n",
    "    time_interval = 0.1\n",
    "    world = PuddleWorld(400000, time_interval, debug=False)  #長時間アニメーション時間をとる\n",
    "\n",
    "    ## 地図を生成して3つランドマークを追加 ##\n",
    "    m = Map()\n",
    "    for ln in [(-4,2), (2,-3), (4,4), (-4,-4)]: m.append_landmark(Landmark(*ln))\n",
    "    world.append(m)   \n",
    "\n",
    "    ##ゴールの追加##\n",
    "    goal = Goal(-3,-3) \n",
    "    world.append(goal)\n",
    "    \n",
    "    ##水たまりの追加##\n",
    "    world.append(Puddle((-2, 0), (0, 2), 0.1)) \n",
    "    world.append(Puddle((-0.5, -2), (2.5, 1), 0.1)) \n",
    "\n",
    "    ##ロボットを1台登場させる##\n",
    "    init_pose = np.array([3, 3, 0]).T\n",
    "    kf = KalmanFilter(m, init_pose)\n",
    "    a = NstepSarsaAgent(time_interval, kf)\n",
    "    r = WarpRobot2(init_pose, sensor=Camera(m, distance_bias_rate_stddev=0, direction_bias_stddev=0), \n",
    "              agent=a, color=\"red\", bias_rate_stds=(0,0))\n",
    "    world.append(r)\n",
    "    \n",
    "    world.draw()\n",
    "    return a\n",
    "    \n",
    "#a = trial()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
